#include "axis.h"
#include "MyProject.h"


/*****************************************************************************/
//轴当前状态
AXIS_State_Type  current_state_;
//轴配置
AXIS_CONFIG  axis_config;

//USB缓冲区
extern  uint8_t  usb_sndbuff[128];
/*****************************************************************************/
//开启闭环控制---初始化
bool start_closed_loop_control(void)
{
	controller_pos_estimate_circular_src_ = &pos_circular_;
	controller_pos_wrap_src_ = &ctrl_config.circular_setpoint_range;
	controller_pos_estimate_linear_src_ = &pos_estimate_;
	controller_vel_estimate_src_ = &vel_estimate_;
	
	// To avoid any transient（瞬态情况） on startup, we intialize the setpoint to be the current position
    
	control_mode_updated();
	input_pos_updated_ = true;
	// Avoid integrator windup issues（避免积分饱和）
	vel_integrator_torque_ = 0.0f;
	//电机扭矩目标设置
	motor_torque_setpoint_src_ = &torque_output_;
	
	//enable_current_control_src_ = (motor_config.motor_type != MOTOR_TYPE_GIMBAL);
	Idq_setpoint_src_ = &motor_Idq_setpoint_;
	Vdq_setpoint_src_ = &motor_Vdq_setpoint_;
    //电角度源
	phase_src_ = &encoder_config.phase_;
	//角速度源
    phase_vel_src_ = &encoder_config.phase_vel_;
	//电机角速度源？
    motor_phase_vel_src_ = &encoder_config.phase_vel_;
	
	// In sensorless mode the motor is already armed.
	if (!is_armed_)
	{
		arm();
	}
	
	return true;
}
/*****************************************************************************/
//开启闭环控制循环
bool run_closed_loop_control_loop(void)
{
    //初始化
	start_closed_loop_control();
	while (is_armed_)
	{delay_us(1000);}
	
	disarm();
	return 0;
}
/*****************************************************************************/
//状态机循环
void run_state_machine_loop(void)
{
	bool status=0;
	uint32_t  len;
	//检查轴当前状态并实施对应操作
	switch(current_state_)
	{
        //轴-电机校准
		case AXIS_STATE_MOTOR_CALIBRATION:{
            //开启校准
			status = run_calibration();   //1表示成功，0表示失败
			//测试
			usb_printf("motor R=%.4f,L=%.4e\r\n", motor_config.phase_resistance,motor_config.phase_inductance);  //电感值比较小，采用科学计数法
			usb_printf("P=%.4f,I=%.4f\r\n", pi_gains_[0],pi_gains_[1]);   //打印整定后的PI参数
			usb_printf("status=%d,error=%X.\r\n", status,motor_error);    //1,0
			//直接进入编码器校准
			if(status)current_state_ = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;  
			else      current_state_ = AXIS_STATE_UNDEFINED;
		} break;
		//轴-编码器零漂校准
		case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: {
            //如果没校准---报错
			if(!is_calibrated_)goto invalid_state_label;
			//进入零漂校准
            status = run_offset_calibration();
			//测试
			usb_printf("dir=%d,offset=%d,float=%.2f\r\n", encoder_config.direction, encoder_config.phase_offset, encoder_config.phase_offset_float);  //串口2打印
			usb_printf("status=%d,error=%X.\r\n", status,motor_error);   //1,0
			//转到未定义状态
			current_state_ = AXIS_STATE_UNDEFINED;
		} break;
		//轴-闭环控制
		case AXIS_STATE_CLOSED_LOOP_CONTROL: {
			if(!is_calibrated_)goto invalid_state_label;
			status = run_closed_loop_control_loop();
		} break;
		//轴-空闲
		case AXIS_STATE_IDLE: {
		} break;
		//轴-未定义
		case AXIS_STATE_UNDEFINED: {
		} break;
		
		default: {
			invalid_state_label:
			set_error(ERROR_INVALID_STATE);
			usb_printf("invalid state!\r\n");   //串口打印
//			len=sprintf((char *)usb_sndbuff, "invalid state!\r\n");  //USB打印
//			usb_send(usb_sndbuff, len);
			status = 0;  // this will set the state to idle
		} break;
	}
	
	// If the state failed, go to idle, else advance task chain
	if (!status)
	{
		current_state_ = AXIS_STATE_IDLE;
	}
//	else
//	{
//		//current_state_++;
//		//current_state_ = AXIS_STATE_UNDEFINED;
//	}
}
/*****************************************************************************/



